home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 46 / Amiga Format CD46 (1999-10-20)(Future Publishing)(GB)[!][issue 1999-12].iso / -in_the_mag- / reader_requests / scilab / demos / pendulum / pendule.dem < prev    next >
Text File  |  1999-09-16  |  3KB  |  152 lines
  1. mode(0)
  2. xselect();xbasc();
  3. //
  4. getf('SCI/demos/pendulum/macros.sci','c')
  5. //
  6. x_message('the cart')
  7. // ----------------
  8. //
  9.  xselect();
  10.  dpnd()
  11. //
  12. // equations 
  13. //----------
  14. //state =[x x' theta theta']  
  15. //
  16.  mb=0.1;mc=1;l=0.3;m=4*mc+mb;//constants
  17. //
  18. x_message('open loop simulation');
  19. //
  20.  y0=[0;0;0.1;0];
  21. // y=ode(y0,0,0.03*(1:180),ivpd);
  22.  y=read('SCI/demos/pendulum/yy',4,180,'(e12.6)');
  23.  xbasc();
  24.  anim(y(1,:),y(3,:),1);
  25. //
  26. x_message('linearization')
  27. //
  28.  x0=[0;0;0;0];u0=0;
  29.  [f,g,h,j]=lin(pendu,x0,u0);
  30.  pe=syslin('c',f,g,h,j);ssprint(pe)
  31. //
  32. x_message('checking the result');
  33. //
  34.  f1=[0 1        0             0
  35.     0 0    -3*mb*9.81/m         0
  36.     0 0        0             1
  37.     0 0  6*(mb+mc)*9.81/(m*l)   0];
  38.  g1=[0 ; 4/m ; 0 ; -6/(m*l)];
  39.  h1=[1 0 0 0
  40.      0 0 1 0];
  41.  norm(f-f1,1)+norm(g-g1,1)+norm(h-h1,1)+norm(j,1)
  42.  
  43. x_message('analysis');
  44. //---------
  45. //unstable system !
  46. //
  47.  spec(f)
  48. //
  49. //controlability
  50. //
  51.  n=contr(f,g)
  52. //
  53. //observability
  54. //
  55.  m1=contr(f',h(1,:)')
  56. //
  57.  [m2,t]=contr(f',h(2,:)')
  58.  
  59. //
  60. x_message('synthesis of a stabilizing controller');
  61. //-------------------------------------------------
  62. //
  63. //pole placement technique
  64. //only x and theta are observed  : contruction of an observer
  65. //to estimate the state : z'=(f-k*h)*z+k*y+g*u
  66. //
  67.  to=0.1;  //
  68.  k=ppol(f',h',-ones(4,1)/to)'  //observer gain
  69. //
  70. //verification
  71. //
  72. // norm( poly(f-k*h,'z')-poly(-ones(4,1)/to,'z'))
  73. //
  74.  kr=ppol(f,g,-ones(4,1)/to)  //compensator gain
  75.  
  76. //
  77. x_message('linear system  pendulum-observer-compensator')
  78. //---------------------------------------------
  79. //
  80. //state: [x x-z]
  81. //
  82.  ft=[f-g*kr            -g*kr
  83.       0*f               f-k*h]
  84.  gt=[g;0*g];
  85.  ht=[h,0*h];
  86.  pr=syslin('c',ft,gt,ht);
  87.  
  88. // closed loop dynamics:
  89.  spec(pr(2))
  90. //transfer matrix representation
  91.  hr=clean(ss2tf(pr),1.d-10)
  92.  
  93.  
  94.  //frequency analysis
  95.  // black(pr,0.01,100,['position','theta'])
  96.  g_margin(pr(1,1))
  97.  
  98.  
  99. //
  100. x_message('sampled system')
  101. //---------------
  102. //
  103.  t=to/5;
  104.  prd=dscr(pr,t);
  105.  spec(prd(2))
  106.  
  107. //
  108. x_message('impulse response')
  109. //-----------------
  110. //
  111.  x0=[0;0;0;0;0;0;0;0];
  112.  u(1,180)=0;u(1,1)=1;
  113.  y=flts(u,prd,x0);
  114. [xxr,yyr]=xgetech();
  115. [xxf]=xget("font");
  116.  xset("wdim",850,650);
  117.  xbasc();
  118.  xset("font",2,0);
  119.  xsetech([0,0,0.5,0.5]);
  120.  plot2d1("enn",1,y(1,:)');xtitle(' ','t',' position');
  121.  xsetech([0.5,0,0.5,0.5]);
  122.  plot2d1("enn",1,y(2,:)');xtitle(' ','t',' theta');
  123.  xsetech([0,0.5,1.0,0.5]);
  124.  anim(100*y(1,:),100*y(2,:),1),
  125.  xsetech(xxr);
  126.  xset("font",xxf(1),xxf(2));
  127. //
  128. x_message('compensation of the non linear system');
  129. //--------------------------------------
  130. //
  131. //simulation
  132. //
  133.  t0=0;t1=t*(1:125);
  134.  x0=[0 0 0.4 0   0 0 0 0]';   //
  135.  yd=ode(x0,t0,t1,regu);
  136. //
  137. x_message('non linear simulation')
  138. [xxr,yyr]=xgetech();
  139. [xxf]=xget("font");
  140.  xbasc();
  141.  xset("font",2,0);
  142.  xsetech([0,0,0.5,0.5]);
  143.  plot2d(t1',(kr*yd(5:8,:))');xtitle(' ','time','control');
  144.  xsetech([0.5,0,0.5,0.5]);
  145.  plot2d([t1;t1]',yd([3,7],:)',[-1,-2],"121",'theta@theta estimated');
  146.  xsetech([0,0.5,1.0,0.5]);
  147.  anim(yd(1,:),yd(3,:),1)
  148.  xsetech(xxr);
  149.  xset("font",xxf(1),xxf(2));
  150.  xbasc();
  151.  xset("wdim",610,455);
  152.